#ifndef _ZR_API_INTERNAL_H_
#define _ZR_API_INTERNAL_H_

#include "spheres_types.h"
#ifdef ZRFLATFLOOR
#include "SpheresSim.h"
#endif

//Termination number if selected competitor ID is not in the function table
#define ZR_RESULT_INVALID_COMPETITOR 254

//ZR Function Pointers
typedef void(*ZR_USR_FCN)(float*,float*,float);
typedef void(*ZR_INIT_FCN)();

//Retrieve user-specified targets for position, velocity, and force.
//THESE FUNCTIONS SHOULD ONLY BE CALLED ONCE PER CYCLE.  After that they will
//be set to 0
unsigned char ZRGetPositionTarget(float *posTarget);
unsigned char ZRGetVelocityTarget(float *velTarget);
unsigned char ZRGetForces(float *forces);
unsigned char ZRGetTorques(float *torques);
unsigned char ZRGetAttitudeTarget(float *attTarget);

/**
 * getZRUserFunctionByIndex
 *
 * Retrieves a function pointer from the function table defined in ZRFunctionTable.c.
 * IT IS EXTREMELY IMPORTANT TO CHECK THE RETURN VALUE TO MAKE SURE YOU ARE NOT CALLING AN
 * INVALID FUNCTION!!!
 */
ZR_USR_FCN getZRUserFunctionByIndex(unsigned char index);

/**
 * getZRInitFunctionByIndex
 *
 * Retrieves a function pointer from the function table defined in ZRFunctionTable.c.
 * IT IS EXTREMELY IMPORTANT TO CHECK THE RETURN VALUE TO MAKE SURE YOU ARE NOT CALLING AN
 * INVALID FUNCTION!!!
 */
ZR_INIT_FCN getZRInitFunctionByIndex(unsigned char index);

/**
 * Translates a runtime command into a team ID.  The assignment uses ascii characters:
 * Character				ID
 * 0x31-0x39				0-8
 * 0x30						9
 * 'q','w','e','r','t'		10-14
 */
unsigned char getZRUserIndexFromCommand(char cmd);

/**
 * ZRAttVec2Quat
 *
 * Finds the quaternion that rotates refVec to attVec.  This
 * is used to determine the quaternion rotation target from a 
 * user unit vector in the global frame.  baseQuat defines the
 * orientation of the satellite when refVec points in the desired
 * direction.  Setting baseQuat to something other than {0,0,0,1} allows
 * the satellite to be rotated around the reference vector.
 */
void ZRAttVec2Quat(float refVec[3], float attVec[3], float baseQuat[4], float quat[4]);

/**
 * ZRQuat2AttVec
 *
 * Rotates the unit vector refVec using quat to determine the direction
 * of attVec.  This is used to tell users where the satellite is pointing.
 */
void ZRQuat2AttVec(float refVec[3], float quat[4], float attVec[3]);

/**
 * ctrlVelocityPgains
 *
 * Performs proportional velocity control using the specified gains
 * for x, y, and z
 *
 * TODO: should be moved to a SpheresCore library
 */
void ctrlVelocityPgains(float kpx, float kpy, float kpz, state_vector ctrlStateError, float ctrlControl[6]);

/**
 * closestIndex
 *
 * returns the index of the component that is closest to zero in a vector of length n
 */
int closestIndex(float refVec[3], int n);

/**
 * dist3d
 *
 * returns the distance between two points given by the vectors (3-component)
 */
float dist3d(float *vecA, float *vecB);


/**
 * Gets the current satellite state and partner state.  If the game is in the 
 * JEM (GAME_AXIS==POS_Y), the coordinate system is rotated to match the simulation view.
 */
void padsStateGetZR(state_vector *ctrlState, state_vector *ctrlStatePartner);

// Functions for 6DOF Flat Floor Simulation
#ifdef ZRFLATFLOOR
//Map tank use to simulation tank use
#define hkpTankUseGetZR sixDOFSimTankUseGet
/**
 * Runs the onboard simulation in the task on each inertial update
 */
void sixDOFSimRun(unsigned int gsp_task_trigger, unsigned int extra_data);
/**
 * Intercepts the firing times and force-torque commands issued by the satellite for 6DOF movement and redirections them
 * to the simulation.  Forces in XY are used for 2D motion, but a PD attitude controller is used to hold heading.
 */
void ZRSimForceTorque(prop_time *firing_times, float *ctrlControl);
#else
//Use normal hkpTankUseGet when not on the flat floor
#define hkpTankUseGetZR hkpTankUseGet
#endif

#endif //End of ZR_API_Internal.h

